#ifndef _CHASSIS_PROTOCOL_H
#define _CHASSIS_PROTOCOL_H

#include "stdint.h"
#include "stdbool.h"
#include "bsp_can.h"

#define FRONT_LEFT_R_ID   (0x01)
#define FRONT_RIGHT_R_ID  (0x03)
#define REAR_LEFT_R_ID    (0x05)
#define REAR_RIGHT_R_ID   (0x07)

#define FRONT_LEFT_S_ID   (0x02)
#define FRONT_RIGHT_S_ID  (0x04)
#define REAR_LEFT_S_ID    (0x09)
#define REAR_RIGHT_S_ID   (0x08)


#define WHEEL_BASE          (0.522f)
//#define VEL_PARAM           (4244132U)
#define VEL_PARAM             (4673494U)
#define DELTA_TO_VEL          (37387.952f) //(3770795U)// /2/VEL_PARAM/0.04

#define CAN_ID_CHASSIS_STATUS   (0x151)
#define CAN_ID_MOTION_STATUS    (0x131)
#define CAN_ID_MOTION_CONTROL   (0x130)


#define MAX_JOYSTICK_VALUE  (660.0f)
#define SPEED_LOW           (0.4f)
#define SPEED_MID           (0.8f)
#define SPEED_HIGHT         (1.0f)

/*motion parameters of scout*/
#define MAX_LIN_VELOCITY       (1.5f)  //1.5m/s
#define MAX_ANG_VELOCITY       (0.7853f) //0.7853rad/s
#define PI                     (3.14159265f)


/*0x151: chassis status filed*/
/*
byte0: chassis status, 00 : ok; 01: error
byte1: control_mode
byte2: battery voltage high byte
byte3: battery voltage low byte
byte4: error info high byte
byte5: error info low byte
byte6: count
byte7: check_sum
*/

/*0x131: motion status filed*/
/*
byte0: linear velocity high byte     (real velocity)*1000
byte1: linear velocity low byte
byte2: angular velocity high byte
byte3: angular velocity low byte
byte4: RSV
byte5: RSV
byte6: count
byte7: check_sum
*/

/*0x130: motion control filed*/
/*
byte0: control_mode
byte1: clear error cmd
byte2: linear_velocity_percentage  xx% of 1.5m/s
byte3: angular_velocity_percentage xx% of 0.7853 rad/s
byte4: RSV
byte5: RSV
byte6: count
byte7: check_sum
*/
enum control_mode
{
    REMOTE_MODE=0,
    CMD_MODE
};

enum clear_err_cmd
{
    NO_CLEAR=0,
    CLEAR_BATTERY_LOW,
    CLEAR_BATTERY_OVER,
    CLEAR_MOTOR1_COMM,
    CLEAR_MOTOR2_COMM,
    CLEAR_MOTOR3_COMM,
    CLEAR_MOTOR4_COMM,
    CLEAR_MOTOR_OVER_TEMP,
    CLEAR_MOTOR_OVER_CURR
};






typedef struct
{
    float front_left;
    float front_right;
    float rear_left;
    float rear_right;
} chassis_detal_speed_t;

typedef struct
{
    uint32_t front_left;
    uint32_t front_right;
    uint32_t rear_left;
    uint32_t rear_right;
} chassis_raw_speed_t;


typedef struct
{
    uint8_t is_enable;
    float max_speed;
    float linear_velocity;
    float angle_velocity;
} joySpeed_t;

typedef struct
{
    uint8_t is_enable;
    float max_speed;
    float linear_velocity;
    float angle_velocity;
} rosCtlChassisSpeed_t;


extern chassis_raw_speed_t chassis_raw_speed;

// ros decode speed
rosCtlChassisSpeed_t ros_decode_msg(userCanBuf_t *canBuf);


chassis_raw_speed_t calc_raw_speed(float linear_velocity,float angle_velocity);
bool send_front_left_speed(chassis_raw_speed_t speed);
bool send_front_right_speed(chassis_raw_speed_t speed);
bool send_rear_left_speed(chassis_raw_speed_t speed);
bool send_rear_right_speed(chassis_raw_speed_t speed);

chassis_raw_speed_t chassis_decode_msg(userCanBuf_t *recvBuf);
rosCtlChassisSpeed_t ros_decode_msg(userCanBuf_t *canBuf);

void chassis_status_package(userCanBuf_t* can_buf,short linear_vel,short angular_vel);

#endif



